#include "hcpp/ptp2_task_queue.h"
//#include "hcpp/ptp2_log.h"

#include "hal/critical_section.h"

#include <string.h>

typedef void (*ptp2_task_queue_reorder_fn)(ptp2_task_queue_t *this, ptp2_task_queue_entry_t *start, ptp2_task_queue_entry_t *current);

static bool                      ptp2_task_queue_setup              (ptp2_task_queue_t *this);
static void                      ptp2_task_queue_destructor         (void *ptr);
static bool                      ptp2_task_queue_add_intern         (ptp2_task_queue_t *this, ptp2_task_queue_priority_t prio, ptp2_task_t *task, bool recurrent, uint32_t timeout);
static void                      ptp2_task_queue_entry_init         (ptp2_task_queue_entry_t *entry);
static ptp2_task_queue_entry_t  *ptp2_task_queue_entry_new          (ptp2_task_t *task, bool recurrent, uint32_t timeout);
static ptp2_task_t              *ptp2_task_queue_pop                (ptp2_task_queue_t *this, ptp2_task_queue_entry_t *start, ptp2_task_queue_reorder_fn ptp2_task_queue_reorder);
static void                      ptp2_task_queue_reorder_timer      (ptp2_task_queue_t *this, ptp2_task_queue_entry_t *start, ptp2_task_queue_entry_t *current);
static void                      ptp2_task_queue_reorder_priority   (ptp2_task_queue_t *this, ptp2_task_queue_entry_t *start, ptp2_task_queue_entry_t *current);

static ptp2_task_queue_t queue;

static class_info_t class_info = {
    .size        = sizeof(ptp2_task_queue_t),
    .destructor  = ptp2_task_queue_destructor,
    .mem_alloc   = NULL,
    .mem_free    = NULL
};

ptp2_task_queue_t *
PTP2_TASK_QUEUE(void)
{
    return &queue;
}

bool
ptp2_task_queue_init(ptp2_task_queue_t *this)
{
    object_init(this, &class_info);
    return ptp2_task_queue_setup(this);
}

static bool
ptp2_task_queue_setup(ptp2_task_queue_t *this)
{
    this->msec = 0;
    ptp2_task_queue_entry_init(&(this->prio_timer));
    ptp2_task_queue_entry_init(&(this->prio_high));
    ptp2_task_queue_entry_init(&(this->prio_medium));
    ptp2_task_queue_entry_init(&(this->prio_low));
    ptp2_task_queue_entry_init(&(this->prio_recurrent));

    return true;
}

static void
ptp2_task_queue_destructor(void *ptr)
{
    //ptp2_task_queue_t *this = ptr;

    // TODO: empty all queues!
}

void
ptp2_task_queue_tick(ptp2_task_queue_t *this, uint32_t msec)
{
    this->msec = msec;
}

bool
ptp2_task_queue_add(ptp2_task_queue_t *this, ptp2_task_queue_priority_t prio, ptp2_task_t *task)
{
    bool recurrent = false;

    if (prio == PTP2_TASK_QUEUE_TIMER) {
        return false;
    } else if (prio == PTP2_TASK_QUEUE_RECURRENT) {
        recurrent = true;
    }

    return ptp2_task_queue_add_intern(this, prio, task, recurrent, 0);
}

bool
ptp2_task_queue_add_timer(ptp2_task_queue_t *this, ptp2_task_t *task, bool recurrent, uint32_t timeout)
{
    return ptp2_task_queue_add_intern(this, PTP2_TASK_QUEUE_TIMER, task, recurrent, timeout);
}

static bool
ptp2_task_queue_add_intern(ptp2_task_queue_t *this, ptp2_task_queue_priority_t prio, ptp2_task_t *task, bool recurrent, uint32_t timeout)
{
    ptp2_task_queue_entry_t     *start;
    ptp2_task_queue_entry_t     *fresh;
    ptp2_task_queue_reorder_fn   ptp2_task_queue_reorder = NULL;

    critical_section_enter();

    /* increment reference counter */
    object_retain(task);

    switch (prio) {
        case PTP2_TASK_QUEUE_TIMER:     start                   = &(this->prio_timer);
                                        ptp2_task_queue_reorder = ptp2_task_queue_reorder_timer;      break;

        case PTP2_TASK_QUEUE_HIGH:      start                   = &(this->prio_high);
                                        ptp2_task_queue_reorder = ptp2_task_queue_reorder_priority;   break;

        case PTP2_TASK_QUEUE_MEDIUM:    start                   = &(this->prio_medium);
                                        ptp2_task_queue_reorder = ptp2_task_queue_reorder_priority;   break;

        case PTP2_TASK_QUEUE_LOW:       start                   = &(this->prio_low);
                                        ptp2_task_queue_reorder = ptp2_task_queue_reorder_priority;   break;

        case PTP2_TASK_QUEUE_RECURRENT: start                   = &(this->prio_recurrent);
                                        ptp2_task_queue_reorder = ptp2_task_queue_reorder_priority;   break;

        default:                        start = NULL;                                                 break;
    }

    /* no appropriate priority found */
    if (start == NULL) {
        goto ptp2_task_queue_add_exit;
    }

    /* allocate new entry or die */
    if ((fresh = ptp2_task_queue_entry_new(task, recurrent, timeout)) == NULL) {
        goto ptp2_task_queue_add_exit;
    }

    /* reorder */
    ptp2_task_queue_reorder(this, start, fresh);

    critical_section_leave();
    return true;

ptp2_task_queue_add_exit:
    object_release(task);
    critical_section_leave();
    return false;
}

/**
 * Initialize only the task queue entries from type `ptp2_task_queue_t`.
 *
 * @param   entry                       The entry to initialize
 */
static void
ptp2_task_queue_entry_init(ptp2_task_queue_entry_t *entry)
{
    entry->prev         = NULL;
    entry->next         = NULL;
    entry->recurrent    = false;
    entry->task         = NULL;
    entry->timeout      = 0;
    entry->msec         = 0;
}

static ptp2_task_queue_entry_t *
ptp2_task_queue_entry_new(ptp2_task_t *task, bool recurrent, uint32_t timeout)
{
    ptp2_task_queue_entry_t *entry;

    //PTP2_LOG_PRINTLN(PTP2_LOG_TASK_QUEUE, PTP2_LOG_DEBUG, ("allocate task queue entry"));
    entry = (ptp2_task_queue_entry_t *) malloc(sizeof(ptp2_task_queue_entry_t));

    if (entry == NULL) {
        //PTP2_LOG_PRINTLN(PTP2_LOG_TASK_QUEUE, PTP2_LOG_DEBUG, ("allocation failed"));
        return NULL;
    }

    entry->prev         = NULL;
    entry->next         = NULL;
    entry->recurrent    = recurrent;
    entry->task         = task;
    entry->timeout      = timeout;

    return entry;
}

ptp2_task_t *
ptp2_task_queue_next(ptp2_task_queue_t *this)
{
    ptp2_task_t *task  = NULL;

    /* high-priority queue */
    if ((task = ptp2_task_queue_pop(this, &(this->prio_high), ptp2_task_queue_reorder_priority))        != NULL) return task;

    /* timer queue */
    if ((task = ptp2_task_queue_pop(this, &(this->prio_timer), ptp2_task_queue_reorder_timer))          != NULL) return task;

    /* medium-priority queue */
    if ((task = ptp2_task_queue_pop(this, &(this->prio_medium), ptp2_task_queue_reorder_priority))      != NULL) return task;

    /* low-priority queue */
    if ((task = ptp2_task_queue_pop(this, &(this->prio_low), ptp2_task_queue_reorder_priority))         != NULL) return task;

    /* recurrent queue */
    if ((task = ptp2_task_queue_pop(this, &(this->prio_recurrent), ptp2_task_queue_reorder_priority))   != NULL) return task;

    return NULL;
}

/**
 *
 * The previously allocated entry of type `task_queue_entry_t` is freed at the end of the function, when it's not markes as recurrent.
 *
 * @param   this                        a
 * @param   start                       a
 * @param   ptp2_task_queue_reorder     a
 * @return                              a
 */
static ptp2_task_t *
ptp2_task_queue_pop(ptp2_task_queue_t *this, ptp2_task_queue_entry_t *start, ptp2_task_queue_reorder_fn ptp2_task_queue_reorder)
{
    ptp2_task_queue_entry_t *entry;
    ptp2_task_t             *task     = NULL;

#if PTP2_LOG
    /*
    if (start->next != NULL && (start != &(this->prio_timer) || start->next->msec <= this->msec)) {

        if      (start == &(this->prio_high))       PTP2_LOG_PRINTLN(PTP2_LOG_TASK_QUEUE, PTP2_LOG_DEBUG, ("Task Queue \"High\""));
        else if (start == &(this->prio_timer))      PTP2_LOG_PRINTLN(PTP2_LOG_TASK_QUEUE, PTP2_LOG_DEBUG, ("Task Queue \"Timer\""));
        else if (start == &(this->prio_medium))     PTP2_LOG_PRINTLN(PTP2_LOG_TASK_QUEUE, PTP2_LOG_DEBUG, ("Task Queue \"Medium\""));
        else if (start == &(this->prio_low))        PTP2_LOG_PRINTLN(PTP2_LOG_TASK_QUEUE, PTP2_LOG_DEBUG, ("Task Queue \"Low\""));
        else if (start == &(this->prio_recurrent))  PTP2_LOG_PRINTLN(PTP2_LOG_TASK_QUEUE, PTP2_LOG_DEBUG, ("Task Queue \"Recurrent\""));

        entry = start->next;
        while (entry != NULL) {
            // PTP2_LOG_PRINTLN(PTP2_LOG_TASK_QUEUE, PTP2_LOG_DEBUG, ("prev=0x%08X current=0x%08X next=0x%08X recurrent=%s task=\"%s\" handler=0x%08X task-refcount=%u",
            //                                                         entry->prev, entry, entry->next, entry->recurrent ? "yes" : "no",
            //                                                         entry->task->name, entry->task->execute, entry->task->obj.ref_count));
            PTP2_LOG_PRINTLN(PTP2_LOG_TASK_QUEUE, PTP2_LOG_DEBUG, ("recurrent=%s time=%u fire=%u task=\"%s\" handler=0x%08X task-refcount=%u",
                                                                    entry->recurrent ? "yes" : "no", this->msec, entry->msec,
                                                                    entry->task->name, entry->task->execute, entry->task->obj.ref_count));
            entry = entry->next;
        }
    }
    */
#endif

    critical_section_enter();

    /*
     *             start
     *          ____________
     *         |            |
     *         |       next |---> NULL
     * NULL <--| prev       |
     *         |____________|
     *
     */

    /* has queue entries? */
    if (start->next != NULL) {

        /**
         *             start             entry
         *          ____________      ____________
         *         |            |    |            |
         *         |       next |--->|       next |---> NULL
         * NULL <--| prev       |<---| prev       |
         *         |____________|    |____________|
         *
         */

        /* take first entry in list */
        entry = start->next;

        /* if it's the timer queue, compare time */
        if (start == &(this->prio_timer) && entry->msec > this->msec) {
            /* time not reached... exit! */
            goto ptp2_task_queue_pop_exit;
        }

        /* take out task */
        task = entry->task;

        /**
         *
         *             start             entry
         *          ____________      ____________      ____________
         *         |            |    |            |    |            |
         *         |       next |--->|       next |--->|       next |--> NULL
         * NULL <--| prev       |<---| prev       |<---| prev       |
         *         |____________|    |____________|    |____________|
         *
         *
         */

        /* additional entries in list? */
        if (entry->next != NULL) {
            /* extract fresh entry and connect start with next entry */
            start->next       = entry->next;
            entry->next->prev = start;
        } else {
            start->next = NULL;
        }

        /* stay in queue? */
        if (entry->recurrent) {

            /* reorder */
            ptp2_task_queue_reorder(this, start, entry);

            /* increase reference counter, because one pointer stays in
               queue and one is returned to the caller */
            object_retain(task);

        } else {
            /* free internal structure */
            free(entry);
        }
    }

ptp2_task_queue_pop_exit:
    critical_section_leave();

#if PTP2_LOG
    if (task != NULL) {
        PTP2_LOG_PRINTLN(PTP2_LOG_TASK_QUEUE, PTP2_LOG_DEBUG, ("task=\"%s\"", task->name));
    }
#endif

    return task;
}

static void
ptp2_task_queue_reorder_timer(ptp2_task_queue_t *this, ptp2_task_queue_entry_t *start, ptp2_task_queue_entry_t *fresh)
{
    ptp2_task_queue_entry_t *current;
    ptp2_task_queue_entry_t *next;
    bool                found = false;

    /* set timeout */
    fresh->msec = this->msec + fresh->timeout; // <== OVERFLOW?

    /* queue is empty */
    if (start->next == NULL) {
        start->next = fresh;
        fresh->prev  = start;

    /* queue is NOT empty */
    } else {
        current = start;
        do {
            current = current->next;
            if (fresh->msec < current->msec) {  // <== OVERFLOW?
                found = true;
                break;
            }
        } while (current->next != NULL);

        /* fresh has earlyer timeout */
        if (found) {
            next          = current;
            current       = current->prev;
            current->next = fresh;
            fresh->prev   = current;
            fresh->next   = next;
            next->prev    = fresh;

        /* fresh has latest timeout: put fresh to tail */
        } else {
            current->next = fresh;
            fresh->prev   = current;
            fresh->next   = NULL;
        }
    }
}

static void
ptp2_task_queue_reorder_priority(ptp2_task_queue_t *this, ptp2_task_queue_entry_t *start, ptp2_task_queue_entry_t *fresh)
{
    ptp2_task_queue_entry_t *current;

    /* queue is empty */
    if (start->next == NULL) {
        start->next = fresh;
        fresh->prev  = start;

    /* queue is NOT empty */
    } else {
        /* put entry at the end of the queue */
        current = start;
        while (current->next != NULL) {
            current = current->next;
        }
        current->next = fresh;
        fresh->prev   = current;
        fresh->next   = NULL;
    }
}




